/**
 * @file bsp_led.c
 * @brief LED driver implementation for H743-V2
 * @author Flight Control Team
 * @date 2025-10-31
 */

#include "bsp_led.h"
#include "main.h"

/* LED GPIO configuration */
typedef struct {
    uint32_t port_base;  // GPIO port base address
    uint16_t pin;        // GPIO pin number
    bool state;          // Current state
    LEDPattern_t pattern;
    uint32_t pattern_timer;
    uint16_t pattern_counter;
} LED_Config_t;

/* GPIO register offsets for STM32H7 */
#define GPIO_MODER_OFFSET   0x00
#define GPIO_OTYPER_OFFSET  0x04
#define GPIO_OSPEEDR_OFFSET 0x08
#define GPIO_PUPDR_OFFSET   0x0C
#define GPIO_ODR_OFFSET     0x14
#define GPIO_BSRR_OFFSET    0x18

/* GPIO port base addresses */
#define GPIOA_BASE 0x58020000UL
#define GPIOB_BASE 0x58020400UL
#define GPIOC_BASE 0x58020800UL
#define GPIOD_BASE 0x58020C00UL
#define GPIOE_BASE 0x58021000UL

/* LED configurations */
static LED_Config_t led_configs[LED_COUNT] = {
    [LED_GREEN] = {GPIOE_BASE, 2, false, {0, 0, 0}, 0, 0},
    [LED_RED]   = {GPIOE_BASE, 3, false, {0, 0, 0}, 0, 0},
    [LED_BLUE]  = {GPIOE_BASE, 4, false, {0, 0, 0}, 0, 0}
};

/* Helper functions */
static inline void gpio_set_mode(uint32_t port, uint16_t pin, uint32_t mode)
{
    volatile uint32_t *moder = (volatile uint32_t *)(port + GPIO_MODER_OFFSET);
    *moder &= ~(0x3UL << (pin * 2));
    *moder |= (mode << (pin * 2));
}

static inline void gpio_set_otype(uint32_t port, uint16_t pin, uint32_t otype)
{
    volatile uint32_t *otyper = (volatile uint32_t *)(port + GPIO_OTYPER_OFFSET);
    *otyper &= ~(0x1UL << pin);
    *otyper |= (otype << pin);
}

static inline void gpio_set_speed(uint32_t port, uint16_t pin, uint32_t speed)
{
    volatile uint32_t *ospeedr = (volatile uint32_t *)(port + GPIO_OSPEEDR_OFFSET);
    *ospeedr &= ~(0x3UL << (pin * 2));
    *ospeedr |= (speed << (pin * 2));
}

static inline void gpio_set_pupd(uint32_t port, uint16_t pin, uint32_t pupd)
{
    volatile uint32_t *pupdr = (volatile uint32_t *)(port + GPIO_PUPDR_OFFSET);
    *pupdr &= ~(0x3UL << (pin * 2));
    *pupdr |= (pupd << (pin * 2));
}

static inline void gpio_write_pin(uint32_t port, uint16_t pin, bool state)
{
    volatile uint32_t *bsrr = (volatile uint32_t *)(port + GPIO_BSRR_OFFSET);
    if (state) {
        *bsrr = (1UL << pin);  // Set bit
    } else {
        *bsrr = (1UL << (pin + 16));  // Reset bit
    }
}

static inline bool gpio_read_pin(uint32_t port, uint16_t pin)
{
    volatile uint32_t *odr = (volatile uint32_t *)(port + GPIO_ODR_OFFSET);
    return (*odr & (1UL << pin)) != 0;
}

/**
 * @brief Initialize LED GPIO
 */
int BSP_LED_Init(void)
{
    /* Enable GPIOE clock (RCC_AHB4ENR bit 4) */
    volatile uint32_t *rcc_ahb4enr = (volatile uint32_t *)0x580244E0UL;
    *rcc_ahb4enr |= (1UL << 4);
    
    /* Small delay for clock to stabilize */
    for (volatile int i = 0; i < 100; i++);
    
    /* Configure each LED pin */
    for (int i = 0; i < LED_COUNT; i++) {
        LED_Config_t *cfg = &led_configs[i];
        
        /* Configure as output (mode = 01) */
        gpio_set_mode(cfg->port_base, cfg->pin, 0x01);
        
        /* Configure as push-pull (otype = 0) */
        gpio_set_otype(cfg->port_base, cfg->pin, 0x00);
        
        /* Configure as medium speed (speed = 01) */
        gpio_set_speed(cfg->port_base, cfg->pin, 0x01);
        
        /* Configure as no pull-up/pull-down (pupd = 00) */
        gpio_set_pupd(cfg->port_base, cfg->pin, 0x00);
        
        /* Turn off LED initially (active low) */
        gpio_write_pin(cfg->port_base, cfg->pin, true);
        cfg->state = false;
    }
    
    return 0;
}

/**
 * @brief Turn on LED
 */
void BSP_LED_On(LED_t led)
{
    if (led >= LED_COUNT) return;
    
    LED_Config_t *cfg = &led_configs[led];
    gpio_write_pin(cfg->port_base, cfg->pin, false);  // Active low
    cfg->state = true;
}

/**
 * @brief Turn off LED
 */
void BSP_LED_Off(LED_t led)
{
    if (led >= LED_COUNT) return;
    
    LED_Config_t *cfg = &led_configs[led];
    gpio_write_pin(cfg->port_base, cfg->pin, true);  // Active low
    cfg->state = false;
}

/**
 * @brief Toggle LED
 */
void BSP_LED_Toggle(LED_t led)
{
    if (led >= LED_COUNT) return;
    
    LED_Config_t *cfg = &led_configs[led];
    cfg->state = !cfg->state;
    gpio_write_pin(cfg->port_base, cfg->pin, !cfg->state);  // Active low
}

/**
 * @brief Set LED state
 */
void BSP_LED_Set(LED_t led, bool state)
{
    if (state) {
        BSP_LED_On(led);
    } else {
        BSP_LED_Off(led);
    }
}

/**
 * @brief Get LED state
 */
bool BSP_LED_GetState(LED_t led)
{
    if (led >= LED_COUNT) return false;
    return led_configs[led].state;
}

/**
 * @brief Set LED pattern
 */
void BSP_LED_SetPattern(LED_t led, const LEDPattern_t *pattern)
{
    if (led >= LED_COUNT || pattern == NULL) return;
    
    LED_Config_t *cfg = &led_configs[led];
    cfg->pattern = *pattern;
    cfg->pattern_timer = 0;
    cfg->pattern_counter = 0;
}

/**
 * @brief Update LED patterns
 */
void BSP_LED_Update(void)
{
    // TODO: Implement pattern update logic
    // This should be called periodically (e.g., every 10ms)
}

/**
 * @brief Turn off all LEDs
 */
void BSP_LED_AllOff(void)
{
    for (int i = 0; i < LED_COUNT; i++) {
        BSP_LED_Off((LED_t)i);
    }
}

/**
 * @brief Turn on all LEDs
 */
void BSP_LED_AllOn(void)
{
    for (int i = 0; i < LED_COUNT; i++) {
        BSP_LED_On((LED_t)i);
    }
}

/**
 * @brief LED test sequence
 */
void BSP_LED_Test(void)
{
    /* Test each LED individually */
    for (int i = 0; i < LED_COUNT; i++) {
        BSP_LED_On((LED_t)i);
        for (volatile uint32_t j = 0; j < 1000000; j++);
        BSP_LED_Off((LED_t)i);
        for (volatile uint32_t j = 0; j < 500000; j++);
    }
    
    /* Test all LEDs together */
    BSP_LED_AllOn();
    for (volatile uint32_t j = 0; j < 2000000; j++);
    BSP_LED_AllOff();
}

